#include "user_global.h"
#include "sentry_robot.h"
#include "can.h"
#include "m3508.h"
#include "gm6020.h"
#include "m2006.h"
#include "pid.h"
#include "adrc.h"
#include "encoder.h"
#include "common_math.h"
#include "referee.h"



/**
*@brief construct sentry robot
* 
*@param 
*/
SentryRobot::SentryRobot(void)
{
    Shoot_Turn_Mode = SingleShoot;//默认是单发模式

    rotation_state = ROTATION_IDLE;//默认空闲状态

    chassis_mode = CHASSIS_SAFE;
    chassis_mode_pre = CHASSIS_SAFE;

    gimbal_mode = GIMBAL_SAFE;
    gimbal_mode_pre = GIMBAL_SAFE;

    shoot_mode = SHOOT_SAFE;
    shoot_mode_pre = SHOOT_SAFE;

    ff_mode = ControlFF;

    // chassis value
    m_chassis_speed = 0.0;//车实际速度大小
    m_chassis_speed2world_angle = 0.0;//车实际速度方向
    m_steer_wheel_gain_k = 1.1;
    m_chassislimit_pow_k = 1;
    m_chassislimit_speedtrack_k = 1;
    m_chassislimit_response_k = 1;

    // gimbal  value
    enemy_find_flag = false;
    gimbal_pitch_des = 0.0;
    gimbal_yaw_des = 0.0;
    yaw_des = 0.0;
    gimbal_pitch_min = 0.0;
    gimbal_pitch_max = 0.0;
    gimbal_yaw_min = 0.0;
    gimbal_yaw_max = 0.0;
    m_vision_delay_cnt = 0;//丢失目标后先保持当前角度再巡逻
    direction_flag = false;//切换巡逻方向
    gimbal_pitch_scan_dir = false;
    gimbal_yaw_scan_dir = false;
    yaw_scan_dir = false;
    yaw_safe_flag = false;
    patrol_flag = 0;
    gimbal_yaw_stepff_flag = 0;


    // shoot   value
    m_shoot_speed_filter  = new EWMA(SHOOT_SPEED_FILTER_BETA);//给辅瞄的滤波弹速
    m_shoot_bullet_cnt_target = 0;
    m_shoot_bullet_fps = 0;
    m_shoot_bullet_last_increase_time = 0;
    shoot_cool_flag = false;
    shoot_driver_angle_target = 0.0;
    shoot_insurance = true;
    m_shoot_speed_pre = 0.0;
    shoot_cool_value = 0;
    friction_speed_down_flag = 1;
    shoot_count = 0;
    shoot_ff_flag = 0;

    Shoot_KEYMOUSE.Key_Left_Time_Cnt= 0;
    Shoot_KEYMOUSE.Key_Lefting_Time_Cnt=0;
    Shoot_KEYMOUSE.Key_Is_Shoot=0;
    Shoot_KEYMOUSE.Key_Is_Assit=0;
    Shoot_KEYMOUSE.Key_Righting_Time_Cnt=0;

    //热量控制结构体初始化
    G_sentry.Shoot_Heat_Control.Heat_Left = 0.0f;//剩余热量 即当前热量与上限差值
    G_sentry.Shoot_Heat_Control.shoot_num = 0.0f;//弹丸数量
    G_sentry.Shoot_Heat_Control.Pre_ShootSpeed = 0.0f;//记录上一次弹丸速度
    G_sentry.Shoot_Heat_Control.Allowed_PelletNum  = 0;  //一个周期内（100ms）实时可发射弹丸数量,受热量限制
    G_sentry.Shoot_Heat_Control.PelletNum = 0;            //已经发射多少颗子弹，每次重载子弹自动清零
    G_sentry.Shoot_Heat_Control.ShooterPosDes_Pre = 0.0f; //记录上一次拨盘目标位置
    

}



/**
*@brief Initial the whole Sentry robot
* 
*@param 
*/
void SentryRobot::
Init(void)
{
    // Initial all actuators
    InitAllActuators();

    // Initial all Sensors
    InitAllSensors();
}


//TODO:【系統】初始化所有電機（包括PID參數、CAN ID、角度範圍等）
/**
*@brief Initial all actuators of the Sentry robot
* 
*@param 
*/
void SentryRobot::InitAllActuators(void)
{
    //chassis_init
    // chassis_steer_motor[CHASSIS_S_BL_MOTOR] = new GM6020(CAN2, CHASSIS_STEER_BL_MOTOR_ID, 1);
    // chassis_steer_motor[CHASSIS_S_BL_MOTOR]->m_angle_td = new Adrc_TD(8000, 0.001, 0.001,0);
    // chassis_steer_motor[CHASSIS_S_BL_MOTOR]->m_angle_pid = new Pid(40, 0.2, 0, 10, 2000, 2000, 5000, 2000);
    // chassis_steer_motor[CHASSIS_S_BL_MOTOR]->m_speed_pid = new Pid(25, 0.00, 0, 10, 28000, 28000, 5000, 2000);
    // chassis_steer_motor[CHASSIS_S_BL_MOTOR]->m_encoder = new AbsEncoder(6199, ENCODER_RESOLUTION); 
    // chassis_steer_motor[CHASSIS_S_BL_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.8f, 0.01f);
    // chassis_steer_motor[CHASSIS_S_BL_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);

    // chassis_steer_motor[CHASSIS_S_BR_MOTOR] = new GM6020(CAN2, CHASSIS_STEER_BR_MOTOR_ID, 1);
    // chassis_steer_motor[CHASSIS_S_BR_MOTOR]->m_angle_td = new Adrc_TD(8000, 0.001, 0.001,0);
    // chassis_steer_motor[CHASSIS_S_BR_MOTOR]->m_angle_pid = new Pid(40, 0.2, 0, 10, 2000, 2000, 5000, 2000);
    // chassis_steer_motor[CHASSIS_S_BR_MOTOR]->m_speed_pid = new Pid(20, 0.00, 0, 10, 28000, 28000, 5000, 2000);
    // chassis_steer_motor[CHASSIS_S_BR_MOTOR]->m_encoder = new AbsEncoder(2730, ENCODER_RESOLUTION); 
    // chassis_steer_motor[CHASSIS_S_BR_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.8f, 0.01f);
    // chassis_steer_motor[CHASSIS_S_BR_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);

    chassis_line_motor[CHASSIS_L_FL_MOTOR] = new M3508(CAN2, CHASSIS_LINE_FL_MOTOR_ID, 13.73);
    chassis_line_motor[CHASSIS_L_FL_MOTOR]->m_angle_td = new Adrc_TD(7000, 0.001, 0.001,0);
    chassis_line_motor[CHASSIS_L_FL_MOTOR]->m_angle_pid = new Pid(0, 0, 0, 0, 2000, 2000, 5000, 2000);
    chassis_line_motor[CHASSIS_L_FL_MOTOR]->m_speed_pid = new Pid(45, 0, 0, 0, 15000, 15000, 5000, 2000);
    chassis_line_motor[CHASSIS_L_FL_MOTOR]->m_encoder = new AbsEncoder(0, ENCODER_RESOLUTION); 
    chassis_line_motor[CHASSIS_L_FL_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.8f, 0.01f);
    chassis_line_motor[CHASSIS_L_FL_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);
    
    chassis_line_motor[CHASSIS_L_FR_MOTOR] = new M3508(CAN2, CHASSIS_LINE_FR_MOTOR_ID, 13.73);
    chassis_line_motor[CHASSIS_L_FR_MOTOR]->m_angle_td = new Adrc_TD(7000, 0.001, 0.001,0);
    chassis_line_motor[CHASSIS_L_FR_MOTOR]->m_angle_pid = new Pid(0, 0, 0, 0, 2000, 2000, 5000, 2000);
    chassis_line_motor[CHASSIS_L_FR_MOTOR]->m_speed_pid = new Pid(45, 0, 0, 0, 15000, 15000, 5000, 2000);
    chassis_line_motor[CHASSIS_L_FR_MOTOR]->m_encoder = new AbsEncoder(0, ENCODER_RESOLUTION); 
    chassis_line_motor[CHASSIS_L_FR_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.8f, 0.01f);
    chassis_line_motor[CHASSIS_L_FR_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);

    chassis_line_motor[CHASSIS_L_BL_MOTOR] = new M3508(CAN2, CHASSIS_LINE_BL_MOTOR_ID, 13.73);
    chassis_line_motor[CHASSIS_L_BL_MOTOR]->m_angle_td = new Adrc_TD(7000, 0.001, 0.001,0);
    chassis_line_motor[CHASSIS_L_BL_MOTOR]->m_angle_pid = new Pid(0, 0, 0, 0, 2000, 2000, 5000, 2000);
    chassis_line_motor[CHASSIS_L_BL_MOTOR]->m_speed_pid = new Pid(55, 0, 0, 0, 15000, 15000, 5000, 2000);
    chassis_line_motor[CHASSIS_L_BL_MOTOR]->m_encoder = new AbsEncoder(0, ENCODER_RESOLUTION); 
    chassis_line_motor[CHASSIS_L_BL_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.8f, 0.01f);
    chassis_line_motor[CHASSIS_L_BL_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);

    chassis_line_motor[CHASSIS_L_BR_MOTOR] = new M3508(CAN2, CHASSIS_LINE_BR_MOTOR_ID, 13.73);
    chassis_line_motor[CHASSIS_L_BR_MOTOR]->m_angle_td = new Adrc_TD(7000, 0.001, 0.001,0);
    chassis_line_motor[CHASSIS_L_BR_MOTOR]->m_angle_pid = new Pid(0, 0, 0, 0, 2000, 2000, 5000, 2000);
    chassis_line_motor[CHASSIS_L_BR_MOTOR]->m_speed_pid = new Pid(55, 0, 0, 0, 15000, 15000, 5000, 2000);
    chassis_line_motor[CHASSIS_L_BR_MOTOR]->m_encoder = new AbsEncoder(0, ENCODER_RESOLUTION); 
    chassis_line_motor[CHASSIS_L_BR_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.8f, 0.01f);
    chassis_line_motor[CHASSIS_L_BR_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);


    //gimbal_init
    gimbal_motor[GIMBAL_YAW_MOTOR] = new GM6020(CAN2, GIMBAL_YAW_MOTOR_ID, GIMBAL_MOTOR_REDUCTION_RATIO);
    gimbal_motor[GIMBAL_YAW_MOTOR]->m_angle_td = new Adrc_TD(400, 0.001f, 0.001f,0.5);//0.1
    gimbal_motor[GIMBAL_YAW_MOTOR]->m_angle_pid = new Pid(90, 0.002, 5, 1, 20000, 20000, 10000, 15000);  // 50 0.5
    gimbal_motor[GIMBAL_YAW_MOTOR]->m_speed_pid = new Pid(65, 0.00, 5, 15, 28000, 28000, 20000, 20000);
    gimbal_motor[GIMBAL_YAW_MOTOR]->m_encoder = new AbsEncoder(GIMBAL_YAW_ENCODER_ZERO_VALUE, ENCODER_RESOLUTION);
    gimbal_motor[GIMBAL_YAW_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.1f, 0.01f);
    gimbal_motor[GIMBAL_YAW_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);


    gimbal_motor[GIMBAL_PITCH_MOTOR] = new GM6020(CAN2, GIMBAL_PITCH_MOTOR_ID, GIMBAL_MOTOR_REDUCTION_RATIO);
    gimbal_motor[GIMBAL_PITCH_MOTOR]->m_angle_td = new Adrc_TD(300, 0.001, 0.001,0);
    gimbal_motor[GIMBAL_PITCH_MOTOR]->m_angle_pid = new Pid(110 , 0, 35, 1, 20000, 16000, 5000, 10000);
    gimbal_motor[GIMBAL_PITCH_MOTOR]->m_speed_pid = new Pid(60, 0, 30, 12, 28000, 28000, 20000, 20000);
    gimbal_motor[GIMBAL_PITCH_MOTOR]->m_encoder = new AbsEncoder(GIMBAL_PITCH_ENCODER_ZERO_VALUE, ENCODER_RESOLUTION);
    gimbal_motor[GIMBAL_PITCH_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.1f, 0.01f);
    gimbal_motor[GIMBAL_PITCH_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);


    gimbal_motor[YAW_MOTOR] = new GM6020(CAN1, YAW_MOTOR_ID, GIMBAL_MOTOR_REDUCTION_RATIO);
    gimbal_motor[YAW_MOTOR]->m_angle_td = new Adrc_TD(1500, 0.001, 0.001,0);
    gimbal_motor[YAW_MOTOR]->m_angle_pid = new Pid(20, 0, 0, 10, 24000, 24000, 10000, 10000);
    gimbal_motor[YAW_MOTOR]->m_speed_pid = new Pid(660, 0.02, 5, 10, 28000, 28000, 20000, 20000);
    gimbal_motor[YAW_MOTOR]->m_encoder = new AbsEncoder(YAW_ENCODER_ZERO_VALUE, ENCODER_RESOLUTION);
    gimbal_motor[YAW_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.1f, 0.01f);
    gimbal_motor[YAW_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);

    //shoot_init
    friction_motor[UP_FRICTION_MOTOR] = new M3508(CAN2,UP_FRIC_MOTOR_ID ,  FRIC_MOTOR_REDUCTION_RATIO);
    friction_motor[UP_FRICTION_MOTOR]->m_smc = new Smc(4.4936132, 3000, 30, 20, 16200);
    friction_motor[UP_FRICTION_MOTOR]->m_smc->m_TD = new Adrc_TD((float)5000, 0.001, 0.001,0);
    friction_motor[UP_FRICTION_MOTOR]->m_encoder = new AbsEncoder(UP_FRIC_MOTOR_ID, ENCODER_RESOLUTION);  
    
    friction_motor[DOWN_FRICTION_MOTOR] = new M3508(CAN2, DOWN_FRIC_MOTOR_ID,  FRIC_MOTOR_REDUCTION_RATIO);
    friction_motor[DOWN_FRICTION_MOTOR]->m_smc = new Smc(4.4936132, 3000, 30, 20, 16200);
    friction_motor[DOWN_FRICTION_MOTOR]->m_smc->m_TD = new Adrc_TD((float)5000, 0.001, 0.001,0);    
    friction_motor[DOWN_FRICTION_MOTOR]->m_encoder = new AbsEncoder(DOWN_FRIC_MOTOR_ID, ENCODER_RESOLUTION);
    
    supply_bullet_motor[SHOOT_DRIVE_MOTOR] = new M2006(CAN1, SUPPLY_BULLET_MOTOR_ID, SHOOT_MOTOR_REDUCTION_RATIO);
    supply_bullet_motor[SHOOT_DRIVE_MOTOR]->m_angle_td = new Adrc_TD(7000, 0.001, 0.001,0);
    supply_bullet_motor[SHOOT_DRIVE_MOTOR]->m_angle_pid = new Pid(40, 0.00, 0, 10, 10000, 10000, 0, 0);
    supply_bullet_motor[SHOOT_DRIVE_MOTOR]->m_speed_pid = new Pid(55, 0.00, 0, 10, 10000, 10000, 0, 0);
    supply_bullet_motor[SHOOT_DRIVE_MOTOR]->m_encoder = new AbsEncoder(0, ENCODER_RESOLUTION);   
    supply_bullet_motor[SHOOT_DRIVE_MOTOR]->m_kalman_filter_angle = new Kalman(1, 0.001f, 0.0001f,0.8f, 0.01f);
    supply_bullet_motor[SHOOT_DRIVE_MOTOR]->m_kalman_filter_speed = new Kalman(1, 0.001f, 0.0001f,0.003f, 0.5f);  

    m_chassis_angle_pid = new Pid(0.66,0,0,5,7,7,0,1);
    m_chassis_resposns_opti_pid = new Pid(0,0,0,0,0,0,0,0);

    InitFeedForward();
}


//TODO：【云台】前馈参数表格初始化、赋值。需要根据实际情况进行调整，感觉也可以自己修改写一个。2.294KG，距离是多少呢？
/**
*@brief 前馈相关参数初始化
* 
*@param 无 话说计算摩擦力前馈函数在哪调用的呢？gm6020.cpp
*/
void SentryRobot::InitFeedForward(void)
{
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table = {.start_angle = -30,.end_angle = 30,.table_length = 7, .gra_table_data = {0}, .fri_table_data = {0}};
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.gra_table_data[0] = 0;    
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.gra_table_data[1] = -500;     
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.gra_table_data[2] = 1000;    
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.gra_table_data[3] = 1200;
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.gra_table_data[4] = 1800;      
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.gra_table_data[5] = 3700;     
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.gra_table_data[6] = 4500;    
		
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.speed_dead_zone = 15;//速度超过15度每秒，进行Fri前馈补偿
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.fri_table_data[0] = 0; 
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.fri_table_data[1] = 0;
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.fri_table_data[2] = 0;
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.fri_table_data[3] = 0;
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.fri_table_data[4] = 0;
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.fri_table_data[5] = 0;
    gimbal_motor[GIMBAL_PITCH_MOTOR]->ff_table.fri_table_data[6] = 0;


    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table = {.start_angle = -80,.end_angle = 0,.table_length = 9, .gra_table_data = {0}, .fri_table_data = {0}};
    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table.gra_table_data[0] = 0; 
    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table.gra_table_data[1] = 0;
    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table.gra_table_data[2] = 0; 
    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table.gra_table_data[3] = 0;
    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table.gra_table_data[4] = 0; 
    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table.gra_table_data[5] = 0;
    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table.gra_table_data[6] = 0; 
    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table.gra_table_data[7] = 0;
    gimbal_motor[GIMBAL_YAW_MOTOR]->ff_table.gra_table_data[8] = 0; 

    gimbal_motor[YAW_MOTOR]->ff_table = {.start_angle = -80,.end_angle = 0,.table_length = 9, .gra_table_data = {0}, .fri_table_data = {0}};
    gimbal_motor[YAW_MOTOR]->ff_table.gra_table_data[0] = 0; 
    gimbal_motor[YAW_MOTOR]->ff_table.gra_table_data[1] = 0;
    gimbal_motor[YAW_MOTOR]->ff_table.gra_table_data[2] = 0; 
    gimbal_motor[YAW_MOTOR]->ff_table.gra_table_data[3] = 0;
    gimbal_motor[YAW_MOTOR]->ff_table.gra_table_data[4] = 0; 
    gimbal_motor[YAW_MOTOR]->ff_table.gra_table_data[5] = 0;
    gimbal_motor[YAW_MOTOR]->ff_table.gra_table_data[6] = 0; 
    gimbal_motor[YAW_MOTOR]->ff_table.gra_table_data[7] = 0;
    gimbal_motor[YAW_MOTOR]->ff_table.gra_table_data[8] = 0; 

    shoot_ff_current_max = 5200;
}


/**
 *@brief Initial all sensors of the Sentry robot
 * 
 *@param 
*/
void SentryRobot::InitAllSensors(void)
{
    capacitor = new Capacitor();
    capacitor->m_power_control_pid = new Pid(1,0.02,0,10,10,10,10,0);
}


//TODO:【底盘】底盘电机控制函数封装
/**
 *@brief execute the sentry robot chassis control algorithm
 * 
 *@param s
*/
void SentryRobot::ExecuteChassisAlgorithm(void)
{
    if (chassis_mode != CHASSIS_SAFE) {
        // for (int i = 0; i < CHASSIS_STEER_MOTOR_NUM; i++) {
        //     chassis_steer_motor[i]->AngleControl();
        // }
        for (int i = 0; i < CHASSIS_LINE_MOTOR_NUM; i++) {
            chassis_line_motor[i]->SpeedControl();
        }
        Power_Control();
    }
}


//TODO:【云台】云台电机控制函数封装
/**
 *@brief execute the sentry robot gimbal control algorithm
 * 
 *@param 
*/
void SentryRobot::ExecuteGimbalAlgorithm(void)
{
    if(gimbal_test_flag)
    {
        for (int i = 0; i < GIMBAL_MOTOR_NUM; i++) {
                    gimbal_motor[i]->AngleControl();
                }
    }
    if (gimbal_mode != GIMBAL_SAFE || chassis_mode == CHASSIS_MANNAL) {
        for (int i = 0; i < GIMBAL_MOTOR_NUM; i++) {
            gimbal_motor[i]->AngleControl();
        }
    }
    if(!yaw_safe_flag)
        gimbal_motor[SentryRobot::YAW_MOTOR]->m_speed_pid->m_output = 0;

    UpdateGimbalPitchFeedForward();
    UpdateGimbalYawFeedForward();
    UpdateYawFeedForward();
}


//TODO:【供弹】供弹电机控制函数封装
/**
 *@brief execute the sentry robot shoot control algorithm
 * 
 *@param 
*/
void SentryRobot::ExecuteShootAlgorithm(void)
{
    if (shoot_mode != SHOOT_SAFE) {
        supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->AngleControl();
        
        if(!shoot_insurance)
        {
            friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->CalSMC();
            friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->CalSMC();
        }
    } 
}


//TODO:【底盘】底盘电机目标速度、目标角度赋值函数封装
/**
 *@brief Set the sentry robot chassis line wheel speed to the specified speed
 * 
 *@param 
*/
void SentryRobot::SetChassisSpeedTarget(float l_fl_motor, float l_bl_motor, float l_fr_motor, float l_br_motor)
{
    chassis_line_motor[CHASSIS_L_FL_MOTOR]->m_speed_target = l_fl_motor;
    chassis_line_motor[CHASSIS_L_BL_MOTOR]->m_speed_target = l_bl_motor;
    chassis_line_motor[CHASSIS_L_FR_MOTOR]->m_speed_target = l_fr_motor;
    chassis_line_motor[CHASSIS_L_BR_MOTOR]->m_speed_target = l_br_motor;
}


//没用
/**
 *@brief Set the sentry robot chassis angle speed to the specified speed
 * 
 *@param 
*/
void SentryRobot::SetChassisAngleSpeedTarget(float s_bl_motor, float s_br_motor)
{
    // chassis_steer_motor[CHASSIS_S_BL_MOTOR]->m_speed_target = s_bl_motor;
    // chassis_steer_motor[CHASSIS_S_BR_MOTOR]->m_speed_target = s_br_motor;

}



/**
 *@brief Set the sentry robot chassis steer angle to the specified angle
 * 
 *@param 
*/
void SentryRobot::SetChassisAngleTarget(float s_bl_motor, float s_br_motor)
{
    // chassis_steer_motor[CHASSIS_S_BL_MOTOR]->m_angle_target = s_bl_motor;
    // chassis_steer_motor[CHASSIS_S_BR_MOTOR]->m_angle_target = s_br_motor;
}



/**
 *@brief Set the sentry robot gimbal angle to the specified angle
 * 
 *@param 
*/
void SentryRobot::SetGimbalAngleTarget(float g_yaw, float g_pitch, float yaw)
{
    gimbal_motor[GIMBAL_YAW_MOTOR]->m_angle_target = g_yaw;
    gimbal_motor[GIMBAL_PITCH_MOTOR]->m_angle_target = g_pitch;
    gimbal_motor[YAW_MOTOR]->m_angle_target = yaw;
}


//TODO:【系統】通過CAN控制所有電機
/**
 *@brief Send control command to all actuators
 * 
 *@param 
*/
void SentryRobot::SendControlCommand(void)
{
    uint8_t transmint_1;
    uint8_t transmint_2;
    // CAN1 ID 0x200    CAN2 ID 0x200
    can1_context.CANx_TxMsg.StdId = 0x200;
    can2_context.CANx_TxMsg.StdId = 0x200;
    for (int i = 0; i < 8; i++) {
        can1_context.CANx_TxMsg.Data[i] = 0;
        can2_context.CANx_TxMsg.Data[i] = 0;
    }

    if(chassis_mode != CHASSIS_SAFE)
    {
        for(int i = 0; i < CHASSIS_LINE_MOTOR_NUM; i++)
            {
                uint8_t m_id;
                uint16_t m_output;

                m_id = chassis_line_motor[i]->m_id;
                m_output = chassis_line_motor[i]->m_speed_pid->m_output;
                if(chassis_line_motor[i]->m_CANx == CAN1)
                {
                    can1_context.CANx_TxMsg.Data[m_id*2-2] = (uint8_t)(m_output >> 8);
                    can1_context.CANx_TxMsg.Data[m_id*2-1] = (uint8_t)(m_output);
                }
                else if(chassis_line_motor[i]->m_CANx == CAN2)
                {
                    can2_context.CANx_TxMsg.Data[m_id*2-2] = (uint8_t)(m_output >> 8);
                    can2_context.CANx_TxMsg.Data[m_id*2-1] = (uint8_t)(m_output);
                }
            }
    }
    

    transmint_1 = can1_context.CanSendMessage();
    transmint_2 = can2_context.CanSendMessage();

    if(transmint_1 == CAN_TxStatus_NoMailBox)
    {   
        can1_context.CANx->TSR|= CAN_TSR_ABRQ0;
        can1_context.CANx->TSR|= CAN_TSR_ABRQ1;
        can1_context.CANx->TSR|= CAN_TSR_ABRQ2;
    }
    if(transmint_2 == CAN_TxStatus_NoMailBox)
    {   
        can2_context.CANx->TSR|= CAN_TSR_ABRQ0;
        can2_context.CANx->TSR|= CAN_TSR_ABRQ1;
        can2_context.CANx->TSR|= CAN_TSR_ABRQ2;
    }

    // CAN1 ID 0x1ff    CAN2 ID 0x1ff
    can1_context.CANx_TxMsg.StdId = 0x1ff;
    can2_context.CANx_TxMsg.StdId = 0x1ff;
    for (int i = 0; i < 8; i++) {
        can1_context.CANx_TxMsg.Data[i] = 0;
        can2_context.CANx_TxMsg.Data[i] = 0;
    }

    // if(chassis_mode != CHASSIS_SAFE)
    // {
    //         for(int i = 0; i < CHASSIS_STEER_MOTOR_NUM; i++)
    //     {
    //         uint8_t m_id;
    //         uint16_t m_output;

    //         m_id = chassis_steer_motor[i]->m_id;
    //         m_output = chassis_steer_motor[i]->m_speed_pid->m_output;
    //         if(chassis_steer_motor[i]->m_CANx == CAN1)
    //         {
    //             can1_context.CANx_TxMsg.Data[m_id*2-2] = (uint8_t)(m_output >> 8);
    //             can1_context.CANx_TxMsg.Data[m_id*2-1] = (uint8_t)(m_output);
    //         }
    //         else if(chassis_steer_motor[i]->m_CANx == CAN2)
    //         {
    //             can2_context.CANx_TxMsg.Data[m_id*2-2] = (uint8_t)(m_output >> 8);
    //             can2_context.CANx_TxMsg.Data[m_id*2-1] = (uint8_t)(m_output);
    //         }
    //     }
    // }

    //yaw motor
    if(gimbal_mode != GIMBAL_SAFE)
    {
        uint32_t id = gimbal_motor[YAW_MOTOR]->m_id;
        int16_t yaw_current = (int16_t)gimbal_motor[YAW_MOTOR]->m_speed_pid->m_output;
        
        int16_t yaw_ff = 0;
        if(G_sentry.ff_mode != SentryRobot::NoFF && !(G_sentry.ff_mode == SentryRobot::ControlFF && G_sentry.gimbal_mode == SentryRobot::GIMBAL_SAFE))
            yaw_ff = G_sentry.yaw_ff_current;

        int16_t cmd = Clip(yaw_current + yaw_ff, -G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_speed_pid->m_output_max, G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_speed_pid->m_output_max);
        
        if(id >= 1 && id <= 4)
        {
            if(gimbal_motor[YAW_MOTOR]->m_CANx == CAN1)
            {
                can1_context.CANx_TxMsg.Data[(id - 1) * 2] = (uint8_t)(cmd >> 8);
                can1_context.CANx_TxMsg.Data[(id - 1) * 2 + 1] = (uint8_t)(cmd);
            }
            else if(gimbal_motor[YAW_MOTOR]->m_CANx == CAN2)
            {
                can2_context.CANx_TxMsg.Data[(id - 1) * 2] = (uint8_t)(cmd >> 8);
                can2_context.CANx_TxMsg.Data[(id - 1) * 2 + 1] = (uint8_t)(cmd);
            }
        }
    }

    // Capacitor power charging target
    // 用于检录
    if(G_control_mode == SAFE) capacitor->CapVolLimit();
    int16_t cmd = (int16_t)capacitor->m_power_charging * 100;
    can1_context.CANx_TxMsg.Data[2] = (uint8_t)( cmd >> 8 );
    can1_context.CANx_TxMsg.Data[3] = (uint8_t)(cmd);
    
    transmint_1 = can1_context.CanSendMessage();
    transmint_2 = can2_context.CanSendMessage();

    if(transmint_1 == CAN_TxStatus_NoMailBox)
    {   
        can1_context.CANx->TSR|= CAN_TSR_ABRQ0;
        can1_context.CANx->TSR|= CAN_TSR_ABRQ1;
        can1_context.CANx->TSR|= CAN_TSR_ABRQ2;
    }
    if(transmint_2 == CAN_TxStatus_NoMailBox)
    {   
        can2_context.CANx->TSR|= CAN_TSR_ABRQ0;
        can2_context.CANx->TSR|= CAN_TSR_ABRQ1;
        can2_context.CANx->TSR|= CAN_TSR_ABRQ2;
    }
}


//TODO:【云台】更新云台pitch、小YAW、大YAW电机角度和速度
/**
 *@brief update the  gimbal pitch motor current angle and current speed
 * 
 *@param 
*/
void SentryRobot::UpdateGimbalPitchState(float angle, float speed)
{
    gimbal_motor[GIMBAL_PITCH_MOTOR]->m_angle_current = angle;
    gimbal_motor[GIMBAL_PITCH_MOTOR]->m_speed_current = speed;
}


//没用
/**
 *@brief update the  yaw motor current angle and current speed
 * 
 *@param 
*/
void SentryRobot::UpdateYawState(float angle, float speed)
{
    gimbal_motor[YAW_MOTOR]->m_angle_current = angle;
    gimbal_motor[YAW_MOTOR]->m_speed_current = speed;
}



/**
 *@brief update the  gimbal yaw motor current angle and current speed
 * 
 *@param 
*/
void SentryRobot::UpdateGimbalYawState(float angle, float speed)
{
    gimbal_motor[GIMBAL_YAW_MOTOR]->m_angle_current = angle;
    gimbal_motor[GIMBAL_YAW_MOTOR]->m_speed_current = speed;
}


//TODO:【发射】发射逻辑 当时间间隔上一次发射时刻满足要求时间时，再次拨弹，并把时间间隔更新
/**
 *@brief Increase shoot bullet count target
 * 
 *@param 
*/
void SentryRobot::IncreaseShootBulletTarget(uint32_t time_now) {
    if (time_now - m_shoot_bullet_last_increase_time > (1000 / m_shoot_bullet_fps)) 
    {
        m_shoot_bullet_cnt_target++;
        m_shoot_bullet_last_increase_time = time_now;
    }
}


//TODO:【发射】拨弹电机角度控制函数封装
/**
 *@brief Set the shoot drive motor position target拨弹电机转角控制
 * 
 *@param angle 目标角度
*/
void SentryRobot::SetShootDriverTarget(float angle) {
    supply_bullet_motor[SHOOT_DRIVE_MOTOR]->m_angle_target = angle;
}

//TODO:【超级电容】功率控制执行器
void SentryRobot::Power_Control(void)
{
    
   if(capacitor->CAP_STATE == 0)//当前功率使用状态未达到需要限制功率时，清零I项
   {
       capacitor->m_power_control_pid->m_error_sum=0;
       capacitor->m_power_control_pid->m_error_pre=0;
   }

    if((capacitor->Pre_CAP_STATE == 1) && (capacitor->CAP_STATE==0)) //如果之前是受限的但是现在不受限，将所有行进轮电机的PID的I项清零
    {
        for(uint8_t i = 0; i < CHASSIS_LINE_MOTOR_NUM; i++)
        {
            chassis_line_motor[i]->m_speed_pid->m_error_sum = 0;
        }
    }

    if(capacitor->CAP_STATE == 1)
    {
        if(capacitor->CAP_POW_USE > 0.5)
        {
            if(capacitor->CAP_POW_USE > 0.9 || capacitor->FLAG_CAP_Pow == 1)
            {
                capacitor->m_power_control_pid->CalWeakenPID(-log(capacitor->CAP_POW_USE)/log(2.7183));//这个PID相对于是以ln(out/limit)等于0作为目标值，把当前的ln(out/limt）作为反馈，使得out趋向于limit
                capacitor->CAP_Cur_Coe = Clip(pow(2.7183,(double)capacitor->m_power_control_pid->m_output),0,1);//que:这个系数会不会太容易等于1了？只有当limit-ou<1/e时,这个数才可能小于1
                m_chassislimit_pow_k = capacitor->CAP_Cur_Coe;//que:这一行是不是有些多余了？
                capacitor->FLAG_CAP_Pow = 1;
                // for(uint8_t i = 0; i < CHASSIS_LINE_MOTOR_NUM; i++)
                // {
                //     chassis_line_motor[i]->m_speed_pid->m_output *= capacitor->CAP_Cur_Coe;
                // }

                float steer_wheel_k = capacitor->CAP_Cur_Coe * m_steer_wheel_gain_k;
                float omni_wheel_k = capacitor->CAP_Cur_Coe / m_steer_wheel_gain_k;
                chassis_line_motor[CHASSIS_L_FL_MOTOR]->m_speed_pid->m_output *= omni_wheel_k;
                chassis_line_motor[CHASSIS_L_FR_MOTOR]->m_speed_pid->m_output *= omni_wheel_k;
                chassis_line_motor[CHASSIS_L_BL_MOTOR]->m_speed_pid->m_output *= steer_wheel_k;
                chassis_line_motor[CHASSIS_L_BR_MOTOR]->m_speed_pid->m_output *= steer_wheel_k;//que:为什么舵机没有乘以一个系数？
            }
        }
        else
        {
            capacitor->FLAG_CAP_Pow = 0;
            capacitor->m_power_control_pid->m_error_pre=0;
            capacitor->m_power_control_pid->m_error_sum=0;
        }
    }

    if(capacitor->CAP_STATE == 2)
    {
        capacitor->m_power_control_pid->m_error_pre=0;
        capacitor->m_power_control_pid->m_error_sum=0;

         for(uint8_t i = 0; i < CHASSIS_LINE_MOTOR_NUM; i++)
        {
            chassis_line_motor[i]->m_speed_pid->m_output = 0;
        }
    }

    capacitor->Pre_CAP_STATE = capacitor->CAP_STATE;
}


//TODO:【底盘】多模式（trackfron、trackdoub、tiger，怎么没有gryo？）目标角度控制
/**
 *@brief 根据不同模式以及目标角度确定真实目标角度
 * 
 *@param float angle_des 目标角度
 *
 * @retval float 真实目标角度
*/
void SentryRobot::SetChassisAngleTarget(float angle_des)
{
    float error1 =RangeFmod(angle_des-m_chassis2world_angle_current,-180.0,180.0);
    float error2 =RangeFmod(angle_des-m_chassis2world_angle_current,-90.0,90.0);
    G_sentry.number_test2 = error2;
    
    if(chassis_w_mode == SpeedTrackFron)
    {   

          m_chassis2world_angle_target = m_chassis2world_angle_current + RangeFmod(angle_des-m_chassis2world_angle_current,-180.0,180.0);

    }
    else if(chassis_w_mode == SpeedTrackDoub)
    {
        
         m_chassis2world_angle_target  = m_chassis2world_angle_current + RangeFmod(angle_des-m_chassis2world_angle_current,-90.0,90.0);

    }
    else if(chassis_w_mode == Tiger)
    {
        static float phase = 0;
        float angle_des_temp = m_chassis2world_angle_target + RangeFmod(angle_des-m_chassis2world_angle_target,-180.0,180.0);
        
        phase += 2*PI/1000/CHASSIS_TIGER_T;
        if(phase > 2*PI)
            phase = 0;

        m_chassis2world_angle_target = angle_des_temp + CHASSIS_TIGER_AMP*sin(phase);
    }
    else if(chassis_w_mode == Follow)
    {
        
         m_chassis2world_angle_target  = m_chassis2world_angle_current + RangeFmod(angle_des-m_chassis2world_angle_current,-90.0,90.0);

    }

    else if(chassis_w_mode == KEYMOUSE_FOLLOW)
    {
        
         m_chassis2world_angle_target  = m_chassis2world_angle_current + RangeFmod(angle_des-m_chassis2world_angle_current,-90.0,90.0);

    }

    else if(chassis_w_mode == KEYMOUSE_SAFE)
    {
        
        m_chassis2world_angle_target = m_chassis2world_angle_current;

    }
    
    else
        m_chassis2world_angle_target = m_chassis2world_angle_current;

}



/**
 *@brief 通过限制底盘目标速度实现功率控制
 * 
 *@param 
*/
void SentryRobot::ChassisPowControl_SpeedSet(uint8_t enable)
{
    
}        


//TODO:【底盘】优化跟随响应
/**
 *@brief 底盘跟随响应优化，减缓打滑现象
 * 
 *@param 
*/
void SentryRobot::ChassisSpeedtrackOptimize(uint8_t enable)
{
    float min_angle = 5;
    float max_angle = 45;
    float min_k = 0.3;

    if(enable)
    {
        float speedtrack_error = fabs(m_chassis2world_angle_target - m_chassis2world_angle_current);
        float temp_k = 1;
        
        if(speedtrack_error < min_angle)
            temp_k = 1;
        else if(speedtrack_error > max_angle)
            temp_k = min_k;
        else
            temp_k = 1 - (speedtrack_error - min_angle) * (1 - min_k) / (max_angle - min_angle);
        
        m_chassislimit_speedtrack_k = m_chassislimit_speedtrack_k + Clip(temp_k - m_chassislimit_speedtrack_k,-1,1.2/1000);
    }
    else
        m_chassislimit_speedtrack_k = 1;
}


//TODO:【底盘】底盘响应优化 
/**
 *@brief 底盘响应优化
 * 
 *@param 
*/
void SentryRobot::ChassisResponseOptimize(uint8_t enable)
{
    if(enable)
    {
        float line_start_speed = 2000, line_k = 1;
        float steer_start_angle = 30, steer_max_angle = 60,steer_k = 1;
        static uint8_t line_control_flag = 0;
        float line_error_max = line_start_speed*0.8;


        //输入
        int i;
        float line_motor_error = 0, steer_motor_error = 0;
        for (i=0; i<4; i++)
        {
            float error = fabs(G_sentry.chassis_line_motor[i]->m_speed_target) - fabs(G_sentry.chassis_line_motor[i]->m_speed_current);
            if(error > 0)
                line_motor_error += error;
        }
        for (i=0; i<2; i++)
            steer_motor_error += fabs(G_sentry.chassis_steer_motor[i]->m_speed_target - G_sentry.chassis_steer_motor[i]->m_speed_current);


        //速度优化
        if(!line_control_flag)
        {
            if(steer_start_angle > line_start_speed)//que:这里条件恒不成立?
                line_control_flag = 1;
            m_chassis_resposns_opti_pid->m_error_sum = 0;
            line_k = 1;
        }
        else
        {
            if(steer_start_angle < 0.5*line_start_speed)
                line_control_flag = 0;
            
            float pid_input = (line_motor_error>line_error_max)*log(line_error_max/line_motor_error);
            m_chassis_resposns_opti_pid->CalWeakenPID(pid_input);
            line_k = exp(m_chassis_resposns_opti_pid->m_output);
            line_k = Clip(line_k,0.5,1);
        }

        //舵向限幅
        if(steer_motor_error < steer_start_angle)
            steer_k = 1;
        else if(steer_motor_error > steer_max_angle)
            steer_k = 0.5;
        else 
            steer_k = 0.5+(steer_motor_error-steer_max_angle)*(1-0.5)*(steer_start_angle-steer_max_angle);

        m_chassislimit_response_k = steer_k*line_k;
    }
    else
        m_chassislimit_response_k = 1;
}



// /**
//  *@brief 修改参数
//  * 
//  *@param 
// */
// void SentryRobot::SetMotorPara(GM6020* p_motor, SentryRobot::GimbalPidPara* p_para)
// {
//     p_motor->m_angle_td->m_k = p_para->td_k;
//     p_motor->m_angle_pid->m_kp = p_para->angle_kp;
//     p_motor->m_angle_pid->m_ki = p_para->angle_ki;
//     p_motor->m_speed_pid->m_kp = p_para->speed_kp;
// }


//TODO:【发射】发射补偿
/**
 *@brief 打弹补偿
 * 
 *@param 
*/
int SentryRobot::GetShootFeedForward(void)
{
    uint8_t delay_time = 15;
    static uint8_t delay_cnt = delay_time;    
    int current_max = -shoot_ff_current_max;
    int shoot_ff_current = 0;
    

    if(shoot_ff_flag)
    {
        delay_cnt = 0;
        shoot_ff_flag = 0;
    }

    if(delay_cnt < delay_time)
    {
        delay_cnt ++;
    } 

    shoot_ff_current = current_max + (0-current_max)*delay_cnt/delay_time;
    return shoot_ff_current;
}


//TODO:【云台】前馈补偿
/**
 *@brief gimabl pitch 补偿
 * 
 *@param 
*/
void SentryRobot::UpdateGimbalPitchFeedForward(void)
{
    int gravity_ff_current = gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->GetGravityFeedForward(gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current_encoder);
    int friction_ff_current = gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->GetFrictionFeedForward(gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current_encoder, gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_speed_current);
    gimbal_pitch_ff_current = gravity_ff_current + friction_ff_current ;
}



/**
 *@brief gimabl yaw 补偿
 * 
 *@param 
*/
void SentryRobot::UpdateGimbalYawFeedForward(void)
{
    int shoot_ff_current = GetShootFeedForward();
    int GimbalYaw_gravity_ff_current = gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->GetGravityFeedForward(gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current_encoder);
    
    gimbal_yaw_ff_current = shoot_ff_current ;
}



/**
 *@brief yaw 补偿
 * 
 *@param 
*/
void SentryRobot::UpdateYawFeedForward(void)
{
    yaw_ff_current = 0;
    int gimbal_yaw_current = 0.5*gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_speed_pid->m_output + gimbal_yaw_ff_current;
    if(fabs(gimbal_yaw_current) > 2000)
    yaw_ff_current += gimbal_yaw_current;

    //int YAW_gravity_ff_current = gimbal_motor[SentryRobot::YAW_MOTOR]->GetGravityFeedForward(gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current_encoder);
    //gimbal_yaw_ff_current = YAW_gravity_ff_current;
}


//TODO:【雷达】
/**
 *@brief 获得雷达角度的旋转偏差(3rad 10;6rad 25)
 * 
 *@param 
*/
float SentryRobot::GetRadarAngleSpinDevCorrect(void)
{
    float dev_correct;
    
    if(fabs(m_chassis_spin_speed) < 0.5)
        dev_correct = 0;
    else if(fabs(m_chassis_spin_speed) < 3)
        dev_correct = (fabs(m_chassis_spin_speed)-0.5) * 10/(3-0.5);
    else if(fabs(m_chassis_spin_speed) < 6)
        dev_correct = 10 + (fabs(m_chassis_spin_speed)-3) * (25-10)/(6-3);
    else
        dev_correct = 25;

    if(m_chassis_spin_speed > 0)
        dev_correct *= -1;

    return dev_correct;
}

/*-------------------------------------------------------------------------------------------------
函数功能：斜坡输入信号(逐步、平滑变化的信号)
-------------------------------------------------------------------------------------------------*/
void SentryRobot::RampSignal(float* p_Output, float DesValue, float Step)
{
    u8 type = 0;

    if(*p_Output < DesValue)
            type = 0;
    else if(*p_Output > DesValue)
            type = 1;

    if(!type)
    {
            if(*p_Output >= DesValue)
            {
                    *p_Output = DesValue;
            }
            else
            {
                    *p_Output += Step;
                    if(*p_Output >= DesValue) 
                        *p_Output = DesValue;
            }
    }
    else
    {
            if(*p_Output <= DesValue)
            {
                    *p_Output = DesValue;
            }
            else
            {
                    *p_Output -= Step;
                    if(*p_Output <= DesValue) *p_Output = DesValue;
            }
    }
    *p_Output = (float)*p_Output;
}


//客户端新建图层UI绘制
void SentryRobot::New_Client(void)
{
    UI_ClientCommand = (UI_Start_Cnt>0)?1:2;
    switch(UI_New_Client_Cnt)
    {
        case 1:
            UI_RefreshCnt++;
            UI_Start_Cnt -= (UI_Start_Cnt>0)?1:0;
          if(UI_RefreshCnt>=3)//每5次操作中，有1次增加4次修改，避免没加上UI
            {
                UI_RefreshCnt = 0;
                UI_Start_Cnt  = 1;
            }
            break;
        case 2:
            Client_Prepare_Normal_Data(UI_ClientCommand);  //示宽线(有一个算法待细看)
            sendPictureToClient(&G_sentry.G_ST_Client_Custom_Graphic_Seven, 7);
			break;
        case 3:
            Client_Draw_Seven_line(UI_ClientCommand);	//一次性画七条直线(图形绘制)
            sendPictureToClient(&G_sentry.G_ST_Client_Custom_Graphic_Seven, 7);
            break;
        case 4:
            Client_Enemy_HP(UI_ClientCommand);  //读取裁判系统的对方当前血量，并绘制在相应的车标下方(数字绘制)
            sendPictureToClient(&G_sentry.G_ST_Client_Custom_Graphic_Five, 5);
            break;
        case 5:
            Client_Prepare_Char_DebugData_1(UI_ClientCommand);	//画一辆mini小车(车上不同颜色的部分分别代表收发正常，收异常，发异常，收发异常)
            sendPictureToClient(&G_sentry.G_ST_Client_Custom_Graphic_Seven, 7);
            break;
        case 6:
            Client_Prepare_Char_DebugData_2(UI_ClientCommand);	//画mini小车
            sendPictureToClient(&G_ST_Client_Custom_Graphic_Seven, 7);
            break;
        //case 7:
        //    Client_Draw_Radar_Message(UI_ClientCommand);//(字符串绘制)
        //    sendPictureToClient(&G_ST_Client_Custom_Character, 3);
		//	break;
        case 7:
            Client_Prepare_Char_Data(UI_ClientCommand);//底盘模式绘制(少量字符绘制)
            sendPictureToClient(&G_ST_Client_Custom_Character, 3);
			break;
        case 8:
            Client_Draw_Console(UI_ClientCommand);//速度表绘制
            sendPictureToClient(&G_ST_Client_Custom_Character, 3);
			break;
        case 9:
            Client_Draw_Aim(UI_ClientCommand);//手动瞄准准星绘制
            sendPictureToClient(&G_sentry.G_ST_Client_Custom_Graphic_Seven, 7);
            break;
        //case 5:
        //      Send_Data_To_Radar();	//给雷达（云台手）发数据
        //      break;
        //case 6:
        //      Send_Data_To_Aerial();	//给飞机（云台手）发数据
        //      break;
        //case 12:
        //    Client_Write_Vision_Status(UI_ClientCommand);	//写视觉状态,no vision或者辅瞄的目标装甲板编号及血量
        //    sendPictureToClient(&G_ST_Client_Custom_Character, 3);
        //    break;
        //case 13:
        //    Client_Draw_Vision_Status(UI_ClientCommand);	//画视觉状态,分为正常辅瞄,大符,小符,反符
        //    sendPictureToClient(&G_ST_Client_Custom_Graphic_Five, 5);
        //    break;
        //case 17:
        //    Client_Draw_AutomaticPatrols();
        //    sendPictureToClient(&G_ST_Client_Custom_Character, 3);
        //    break;
        default:
            UI_New_Client_Cnt = 0;	//从头开始
            break;
    }
    UI_New_Client_Cnt++;
}

/*------------------------------------------------------------------------
函 数 名：void Picture_Operation(graphic_data_struct_t* pGraphicData, u8* graphicName, u32 operateTpye, u32 graphicTpye,
                                u32 layer, u32 color, u32 startAngle, u32 endAngle, u32 width, u32 startX, u32 startY,
                                u32 radius, u32 endX, u32 endY)
函数功能：对图像进行操作
------------------------------------------------------------------------*/
u8 Custom_DataBuf[128] = {0};
u16 Custom_Data_Length = NULL;
u16 Des_ID = NULL;
u16 Soldier_USE_ID = 0x203;//0x0200~0x02FF
u16 Friend_ID = NULL;
void SentryRobot::pictureOperation(graphic_data_struct_t* pGraphicData, u8* graphicName, u32 operateTpye, u32 graphicTpye,
                      u32 layer, u32 color, u32 startAngle, u32 endAngle, u32 width, u32 startX, u32 startY,
                      u32 radius, u32 endX, u32 endY)
{
    if( ( pGraphicData == NULL ) || ( graphicName == NULL ) )
    {
        return;
    }

    memcpy(pGraphicData->graphic_name, graphicName, 3); //图形名
    pGraphicData->operate_tpye = operateTpye; //图形操作
    pGraphicData->graphic_tpye = graphicTpye; //图形类型
    pGraphicData->layer = layer; //图层数，0-9
    pGraphicData->color = color; //颜色
    pGraphicData->start_angle = startAngle; //起始角度，单位：°，范围[0,360]
    pGraphicData->end_angle = endAngle; //终止角度，单位：°，范围[0,360]
    pGraphicData->width = width; //线宽
    pGraphicData->start_x = startX; //起点 x 坐标
    pGraphicData->start_y = startY; //起点 y 坐标
    pGraphicData->radius = radius; //字体大小或者半径
    pGraphicData->end_x = endX; //终点 x 坐标
    pGraphicData->end_y = endY; //终点 x 坐标
}

/*------------------------------------------------------------------------
函 数 名：void sendPictureToClient(void* picture, u16 pictureNum)
函数功能：向客户端发送自定义图像
------------------------------------------------------------------------*/
u8  picture1[105];
void SentryRobot::sendPictureToClient(void* picture, u16 pictureNum)
{
    u16 dataLength = 6 + pictureNum * 15;

    switch(pictureNum) //命令ID
    {
    case 1:
    G_sentry.G_ST_Student_Interactive_Header_Data.data_cmd_id = 0x0101;
      break;
    case 2:
    G_sentry.G_ST_Student_Interactive_Header_Data.data_cmd_id = 0x0102;
      break;
    case 5:
    G_sentry.G_ST_Student_Interactive_Header_Data.data_cmd_id = 0x0103;
      break;
    case 7:
    G_sentry.G_ST_Student_Interactive_Header_Data.data_cmd_id = 0x0104;
      break;
        case 3://客户端绘制字符
    G_sentry.G_ST_Student_Interactive_Header_Data.data_cmd_id = 0x0110;
      break;
    default:
      return;
      break;
  }
    
  uint16_t data_length;
  uint16_t cmd_id;

  data_length  = 6 + pictureNum * 15;
  G_sentry.G_ST_Student_Interactive_Header_Data.sender_ID = G_referee.GameRobotStatus.robot_id; //发送方ID
  G_sentry.G_ST_Student_Interactive_Header_Data.receiver_ID = G_referee.GameRobotStatus.robot_id + 0x100;
  cmd_id=0x0301;

  //数据长度
  //G_referee.m_data_send_buff[0] = 0xA5;
  G_referee.m_data_send_buff[1] = (uint8_t) data_length;
  G_referee.m_data_send_buff[2] = (uint8_t) (data_length >> 8);
  //G_referee.m_data_send_buff[3] = 0x00;
  Append_CRC8_Check_Sum(G_referee.m_data_send_buff,5);
  //命令码
  G_referee.m_data_send_buff[5] = (uint8_t) cmd_id;
  G_referee.m_data_send_buff[6] = (uint8_t) (cmd_id >> 8);
  memcpy(&G_referee.m_data_send_buff[7], &G_sentry.G_ST_Student_Interactive_Header_Data, 6);
  memcpy(&G_referee.m_data_send_buff[13], picture, pictureNum * 15);//一定注意字节对齐
  //发送
  Append_CRC16_Check_Sum(G_referee.m_data_send_buff, data_length + 9);//TODO:【通讯】CRC校验码的具体实践？
  G_referee.SendData(data_length + 9);
  
}

u8 UI_Chassis_1[4] = "Ch1";
u8 UI_Chassis_2[4] = "Ch2";
u8 UI_Chassis_3[4] = "Ch3";
u8 UI_Chassis_4[4] = "Ch4";
u8 UI_Chassis_5[4] = "Ch5";
u8 UI_Chassis_6[4] = "Ch6";
u8 UI_Chassis_7[4] = "Ch7";
u8 UI_Chassis_8[4] = "Ch8";
u8 UI_Chassis_9[4] = "Ch9";
u8 UI_Chassis_10[4] = "ChA";
u8 UI_Chassis_11[4] = "ChB";
u8 UI_Chassis_12[4] = "ChC";
u8 UI_Chassis_13[4] = "ChD";
u8 UI_Chassis_14[4] = "ChE";
u8 *UI_Chassis[] = {UI_Chassis_1, UI_Chassis_2, UI_Chassis_3, UI_Chassis_4, UI_Chassis_5, UI_Chassis_6, UI_Chassis_7,
                        UI_Chassis_8, UI_Chassis_9, UI_Chassis_10, UI_Chassis_11, UI_Chassis_12, UI_Chassis_13, UI_Chassis_14};
uint32_t ui_chassis_x_base = 960;
uint32_t ui_chassis_y_base = 325;
uint32_t ui_chassis_length = 200;
uint32_t ui_chassis_width = 150;
uint32_t ui_chassis_radius = 125;

void SentryRobot::Client_Draw_Seven_line(u8 Command)
{
 
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[0], UI_Chassis[0], Command, /*直线*/0,  2,  0/*己方颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_chassis_x_base, /*起点坐标Y*/ui_chassis_y_base+50, 0/*无*/, ui_chassis_x_base-50/*终点坐标x*/, ui_chassis_y_base-50/*终点坐标y*/);
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[1], UI_Chassis[1], Command, /*直线*/0,  2,  0/*己方颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_chassis_x_base-50, /*起点坐标Y*/ui_chassis_y_base-50, 0/*无*/, ui_chassis_x_base-75/*终点坐标x*/, ui_chassis_y_base/*终点坐标y*/);
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[2], UI_Chassis[2], Command, /*直线*/0,  2,  0/*己方颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_chassis_x_base-75, /*起点坐标Y*/ui_chassis_y_base, 0/*无*/, ui_chassis_x_base-125/*终点坐标x*/, ui_chassis_y_base-100/*终点坐标y*/);
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[3], UI_Chassis[3], Command, /*直线*/0,  2,  0/*己方颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_chassis_x_base-75, /*起点坐标Y*/ui_chassis_y_base-80, 0/*无*/, ui_chassis_x_base+75/*终点坐标x*/, ui_chassis_y_base-80/*终点坐标y*/);
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[4], UI_Chassis[4], Command, /*直线*/0,  2,  0/*己方颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_chassis_x_base, /*起点坐标Y*/ui_chassis_y_base+50, 0/*无*/, ui_chassis_x_base+50/*终点坐标x*/, ui_chassis_y_base-50/*终点坐标y*/);
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[5], UI_Chassis[5], Command, /*直线*/0,  2,  0/*己方颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_chassis_x_base+50, /*起点坐标Y*/ui_chassis_y_base-50, 0/*无*/, ui_chassis_x_base+75/*终点坐标x*/, ui_chassis_y_base/*终点坐标y*/);
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[6], UI_Chassis[6], Command, /*直线*/0,  2,  0/*己方颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_chassis_x_base+75, /*起点坐标Y*/ui_chassis_y_base, 0/*无*/, ui_chassis_x_base+125/*终点坐标x*/, ui_chassis_y_base-100/*终点坐标y*/);

}
u8 UI_aim_1[4] = "AM1";
u8 UI_aim_2[4] = "AM2";
u8 UI_aim_3[4] = "AM3";
u8 UI_aim_4[4] = "AM4";
u8 UI_aim_5[4] = "AM5";
u8 UI_aim_6[4] = "AM6";
u8 UI_aim_7[4] = "AM7";
u8 UI_aim_8[4] = "AM8";
u8 UI_aim_9[4] = "AM9";

u8 *UI_aim[] = {UI_aim_1, UI_aim_2, UI_aim_3, UI_aim_4, UI_aim_5, UI_aim_6, UI_aim_7,
                        UI_aim_8, UI_aim_9};

uint32_t ui_aim_x_base=700;
uint32_t ui_aim_y_base=300;
void SentryRobot::Client_Draw_Aim(u8 Command)
{
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[0], UI_aim[0], Command, /*直线*/0,  2,  0/*己方颜色*/, 
        /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_aim_x_base+75, /*起点坐标Y*/ui_aim_y_base+75+300, 0/*无*/, ui_aim_x_base+25/*终点坐标x*/, ui_aim_y_base+25+300/*终点坐标y*/);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[1], UI_aim[1], Command, /*直线*/0,  2,  0/*己方颜色*/, 
        /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_aim_x_base+75, /*起点坐标Y*/ui_aim_y_base-75+300, 0/*无*/, ui_aim_x_base+25/*终点坐标x*/, ui_aim_y_base-25+300/*终点坐标y*/);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[2], UI_aim[2], Command, /*直线*/0,  2,  0/*己方颜色*/, 
        /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_aim_x_base-75, /*起点坐标Y*/ui_aim_y_base-75+300, 0/*无*/, ui_aim_x_base-25/*终点坐标x*/, ui_aim_y_base-25+300/*终点坐标y*/);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[3], UI_aim[3], Command, /*直线*/0,  2,  0/*己方颜色*/, 
        /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_aim_x_base-75, /*起点坐标Y*/ui_aim_y_base+75+300, 0/*无*/, ui_aim_x_base-25/*终点坐标x*/, ui_aim_y_base+25+300/*终点坐标y*/);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[4], UI_aim[4], Command, /*正圆*/2,  2,  0/*己方颜色*/, 
        /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_aim_x_base, /*起点坐标Y*/ui_aim_y_base+300, 10/*无*/, 0, 0);
    
    if(G_sentry.gyro_control_flag == 1)
    {
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[5], UI_aim[5], Command, /*直线*/0,  2,  0/*颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/6, /*起点坐标X*/ui_aim_x_base+90, /*起点坐标Y*/ui_aim_y_base+300, 0/*无*/, ui_aim_x_base+15/*终点坐标x*/, ui_aim_y_base+300/*终点坐标y*/);
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[6], UI_aim[6], Command, /*直线*/0,  2,  0/*颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/6, /*起点坐标X*/ui_aim_x_base-90, /*起点坐标Y*/ui_aim_y_base+300, 0/*无*/, ui_aim_x_base-15/*终点坐标x*/, ui_aim_y_base+300/*终点坐标y*/);
    }
    else
    {
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[5], UI_aim[5], Command, /*直线*/0,  2,  1/*颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/6, /*起点坐标X*/ui_aim_x_base+90, /*起点坐标Y*/ui_aim_y_base+300, 0/*无*/, ui_aim_x_base+15/*终点坐标x*/, ui_aim_y_base+300/*终点坐标y*/);
        pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[6], UI_aim[6], Command, /*直线*/0,  2,  1/*颜色*/, 
            /*无*/0, /*无*/0, /*线宽*/6, /*起点坐标X*/ui_aim_x_base-90, /*起点坐标Y*/ui_aim_y_base+300, 0/*无*/, ui_aim_x_base-15/*终点坐标x*/, ui_aim_y_base+300/*终点坐标y*/);
   
    }
        
}
//在界面上显示底盘运动模式
u8 Char1Name[4] = "CH1";
u8 Char2Name[4] = "CH2";
u8 Char3Name[4] = "CH3";
u8 Char4Name[4] = "CH4";
u8 Char5Name[4] = "CH5";
u8 Char6Name[4] = "CH6";
u8* CharName[] = {Char1Name, Char2Name, Char3Name, Char4Name, Char5Name, Char6Name};

                  /*123456 789 012 345 678 90123456789*/
u8 Send_Char[30] = "     \n  \n  \n  \n  \n           ";
void SentryRobot::Client_Prepare_Char_Data(u8 Command)
{
    {
    int i;
//	int Bullet_Num;
    while( i <= 29 )
    {
        G_ST_Client_Custom_Character.data[i] = Send_Char[i];
        i++;
    }
    
    {//底盘：1 2 3 4
        if(G_sentry.chassis_mode == SentryRobot::CHASSIS_MANNAL) 
        {
            G_ST_Client_Custom_Character.data[0] = '"';
            G_ST_Client_Custom_Character.data[1] = 'M';
            G_ST_Client_Custom_Character.data[2] = 'A';
            G_ST_Client_Custom_Character.data[3] = 'N';
            G_ST_Client_Custom_Character.data[4] = 'N';
            G_ST_Client_Custom_Character.data[5] = ';';
        }
        else if(G_sentry.chassis_mode == SentryRobot::CHASSIS_SAFE)
        {
            G_ST_Client_Custom_Character.data[0] = '"';
            G_ST_Client_Custom_Character.data[1] = 'S';
            G_ST_Client_Custom_Character.data[2] = 'A';
            G_ST_Client_Custom_Character.data[3] = 'F';
            G_ST_Client_Custom_Character.data[4] = 'E';
            G_ST_Client_Custom_Character.data[5] = ';';
        }
        else if(G_sentry.chassis_mode == SentryRobot::CHASSIS_TEST)
        {
            G_ST_Client_Custom_Character.data[0] = '"';
            G_ST_Client_Custom_Character.data[1] = 'T';
            G_ST_Client_Custom_Character.data[2] = 'E';
            G_ST_Client_Custom_Character.data[3] = 'S';
            G_ST_Client_Custom_Character.data[4] = 'T';
            G_ST_Client_Custom_Character.data[5] = ';';
        }
        else if(G_sentry.chassis_mode == SentryRobot::CHASSIS_AUTO)
        {
            G_ST_Client_Custom_Character.data[0] = '"';
            G_ST_Client_Custom_Character.data[1] = 'A';
            G_ST_Client_Custom_Character.data[2] = 'U';
            G_ST_Client_Custom_Character.data[3] = 'T';
            G_ST_Client_Custom_Character.data[4] = 'O';
            G_ST_Client_Custom_Character.data[5] = ';';
        }
        else if(G_sentry.chassis_mode == SentryRobot::CHASSIS_KEYMOUSE)
        {
            G_ST_Client_Custom_Character.data[0] = '"';
            G_ST_Client_Custom_Character.data[1] = 'K';
            G_ST_Client_Custom_Character.data[2] = 'E';
            G_ST_Client_Custom_Character.data[3] = 'Y';
            G_ST_Client_Custom_Character.data[4] = 'M';
            G_ST_Client_Custom_Character.data[5] = ';';
        }
        else
        {
            G_ST_Client_Custom_Character.data[0] = '"';
            G_ST_Client_Custom_Character.data[1] = 'E';
            G_ST_Client_Custom_Character.data[2] = 'L';
            G_ST_Client_Custom_Character.data[3] = 'S';
            G_ST_Client_Custom_Character.data[4] = 'E';
            G_ST_Client_Custom_Character.data[5] = ';';
        }
    }

    {
        if(G_sentry.gimbal_mode== SentryRobot::GIMBAL_MANNAL) 
        {
            G_ST_Client_Custom_Character.data[6] = 'M';
            G_ST_Client_Custom_Character.data[7] = 'A';
            G_ST_Client_Custom_Character.data[8] = 'N';
            G_ST_Client_Custom_Character.data[9] = 'N';
            G_ST_Client_Custom_Character.data[10] = ';';
        }
        else if(G_sentry.gimbal_mode == SentryRobot::GIMBAL_SAFE)
        {
            G_ST_Client_Custom_Character.data[6] = 'S';
            G_ST_Client_Custom_Character.data[7] = 'A';
            G_ST_Client_Custom_Character.data[8] = 'F';
            G_ST_Client_Custom_Character.data[9] = 'E';
            G_ST_Client_Custom_Character.data[10] = ';';
        }
        else if(G_sentry.gimbal_mode == SentryRobot::GIMBAL_AUTO)
        {
            G_ST_Client_Custom_Character.data[6] = 'A';
            G_ST_Client_Custom_Character.data[7] = 'U';
            G_ST_Client_Custom_Character.data[8] = 'T';
            G_ST_Client_Custom_Character.data[9] = 'O';
            G_ST_Client_Custom_Character.data[10] = ';';
        }
        else if(G_sentry.gimbal_mode == SentryRobot::GIMBAL_KEYMOUSE)
        {
            G_ST_Client_Custom_Character.data[6] = 'K';
            G_ST_Client_Custom_Character.data[7] = 'E';
            G_ST_Client_Custom_Character.data[8] = 'Y';
            G_ST_Client_Custom_Character.data[9] = 'M';
            G_ST_Client_Custom_Character.data[10] = ';';
        }
        else
        {
            G_ST_Client_Custom_Character.data[6] = 'E';
            G_ST_Client_Custom_Character.data[7] = 'L';
            G_ST_Client_Custom_Character.data[8] = 'S';
            G_ST_Client_Custom_Character.data[9] = 'E';
            G_ST_Client_Custom_Character.data[10] = ';';
        }

    }
    {
        if(G_sentry.shoot_mode== SentryRobot::SHOOT_MANNAL) 
        {
            G_ST_Client_Custom_Character.data[11] = 'M';
            G_ST_Client_Custom_Character.data[12] = 'A';
            G_ST_Client_Custom_Character.data[13] = 'N';
            G_ST_Client_Custom_Character.data[14] = 'N';
            G_ST_Client_Custom_Character.data[15] = '"';
        }
        else if(G_sentry.shoot_mode == SentryRobot::SHOOT_SAFE)
        {
            G_ST_Client_Custom_Character.data[11] = 'S';
            G_ST_Client_Custom_Character.data[12] = 'A';
            G_ST_Client_Custom_Character.data[13] = 'F';
            G_ST_Client_Custom_Character.data[14] = 'E';
            G_ST_Client_Custom_Character.data[15] = '"';
            
        }
        else if(G_sentry.shoot_mode == SentryRobot::SHOOT_AUTO)
        {
            G_ST_Client_Custom_Character.data[11] = 'A';
            G_ST_Client_Custom_Character.data[12] = 'U';
            G_ST_Client_Custom_Character.data[13] = 'T';
            G_ST_Client_Custom_Character.data[14] = 'O';
            G_ST_Client_Custom_Character.data[15] = '"';
            
        }
        else if(G_sentry.shoot_mode == SentryRobot::SHOOT_HALFAUTO)
        {
            G_ST_Client_Custom_Character.data[11] = 'H';
            G_ST_Client_Custom_Character.data[12] = 'A';
            G_ST_Client_Custom_Character.data[13] = 'L';
            G_ST_Client_Custom_Character.data[14] = 'F';
            G_ST_Client_Custom_Character.data[15] = '"';     
        }
        else if(G_sentry.shoot_mode == SentryRobot::SHOOT_KEYMOUSE)
        {
            G_ST_Client_Custom_Character.data[11] = 'K';
            G_ST_Client_Custom_Character.data[12] = 'E';
            G_ST_Client_Custom_Character.data[13] = 'Y';
            G_ST_Client_Custom_Character.data[14] = 'M';
            G_ST_Client_Custom_Character.data[15] = '"';     
        }
        else
        {
            G_ST_Client_Custom_Character.data[11] = 'E';
            G_ST_Client_Custom_Character.data[12] = 'L';
            G_ST_Client_Custom_Character.data[13] = 'S';
            G_ST_Client_Custom_Character.data[14] = 'E';
            G_ST_Client_Custom_Character.data[15] = '"';
            
        }

    }
    
    }
    pictureOperation(&G_ST_Client_Custom_Character.grapic_data_struct, CharName[0], Command, 7, 6, en_Green,
                         /*字体大小*/20, /*字符长度*/30, /*线宽*/2, /*起点坐标X*/1290, /*起点坐标Y*/770 , 0,0,0);
    }


uint32_t ui_x_debug_color = 100;
uint32_t ui_y_debug_color = 700;
SentryRobot::EN_Client_PictureColor ui_color ;
//根据system_monitor的接收和发送帧率来决定颜色，然后再绘制一辆mini小车，各模块的颜色取决于帧率正不正常
u8 UI_Debug_0[4] = "D00";
u8 UI_Debug_1[4] = "D01";
u8 UI_Debug_2[4] = "D02";
u8 UI_Debug_3[4] = "D03";
u8 UI_Debug_4[4] = "D04";
u8 UI_Debug_5[4] = "D05";
u8 UI_Debug_6[4] = "D06";
u8 UI_Debug_7[4] = "D07";
u8 UI_Debug_8[4] = "D08";
u8 UI_Debug_9[4] = "D09";
u8 UI_Debug_10[4] = "D10";
u8 UI_Debug_11[4] = "D11";
u8 UI_Debug_12[4] = "D12";
u8 UI_Debug_13[4] = "D13";
u8 UI_Debug_14[4] = "D14";
u8 UI_Debug_15[4] = "D15";
u8 UI_Debug_16[4] = "D16";
u8 UI_Debug_17[4] = "D17";
u8 UI_Debug_18[4] = "D18";
u8 UI_Debug_19[4] = "D19";
u8 UI_Debug_20[4] = "D20";
u8 *UI_Debug[] = {UI_Debug_0, UI_Debug_1, UI_Debug_2, UI_Debug_3, UI_Debug_4, UI_Debug_5, UI_Debug_6,\
                     UI_Debug_7, UI_Debug_8, UI_Debug_9, UI_Debug_10, UI_Debug_11, UI_Debug_12, UI_Debug_13,\
                     UI_Debug_14, UI_Debug_15, UI_Debug_16, UI_Debug_17, UI_Debug_18, UI_Debug_19, UI_Debug_20};
void SentryRobot::Client_UIColor_Decision(u16 data_rx, u16 target_rx, u16 data_tx, u16 target_tx) //根据收发帧率决定mini小车的相应部分的颜色
{
    if( data_rx>=target_rx && data_tx>=target_tx )//收发正常,绿色
    {	ui_color = en_Green;	}
    else if( data_rx<target_rx && data_tx>=target_tx )//发正常,收不正常,橙色
    {	ui_color = en_Orange;	}
    else if( data_rx>=target_rx && data_tx<target_tx ) //收正常,发不正常,粉色
    {	ui_color = en_Pink;	}
    else //收发都不正常,黑色
    {	ui_color = en_Black;	}
}

void SentryRobot::Client_Prepare_Char_DebugData_1(u8 Command)//
{
    //先画车体-矩形
    pictureOperation(&G_sentry.G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[0], UI_Debug[0], Command, /*矩形*/1,  8,  en_Green, 
        /*无*/0, /*无*/0, /*线宽*/2, /*起点坐标X*/ui_x_debug_color, /*起点坐标Y*/ui_y_debug_color, 0/*无*/, ui_x_debug_color+200/*对角顶点x*/, ui_y_debug_color+50/*对角顶点y*/);
    
        
    //画左前轮-圆弧
    Client_UIColor_Decision(G_system_monitor.CAN1_rx_fps, 0, G_system_monitor.CAN1_rx_fps, 0);
    pictureOperation(&G_sentry.G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[1], UI_Debug[1], Command, /*圆弧*/4,  8,  en_Green, 
        /*起始角度*/90, /*终止角度*/210, /*线宽*/2, /*圆心坐标X*/ui_x_debug_color+160, /*圆心坐标Y*/ui_y_debug_color, 0/*无*/, 20/*x半轴长度*/, 20/*y半轴长度*/);

    //画左后轮-圆弧
    Client_UIColor_Decision(G_system_monitor.CAN1_rx_fps, 0, G_system_monitor.CAN1_rx_fps, 0);
    pictureOperation(&G_sentry.G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[2], UI_Debug[2], Command, /*圆弧*/4,  8,  en_Green, 
        /*起始角度*/90, /*终止角度*/210, /*线宽*/2, /*圆心坐标X*/ui_x_debug_color+60, /*圆心坐标Y*/ui_y_debug_color, 0/*无*/, 20/*x半轴长度*/, 20/*y半轴长度*/);

    //画右后轮-圆弧
    Client_UIColor_Decision(G_system_monitor.CAN2_rx_fps, 0, G_system_monitor.CAN2_rx_fps, 0);
    pictureOperation(&G_sentry.G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[3], UI_Debug[3], Command, /*圆弧*/4,  8,  en_Green, 
        /*起始角度*/90, /*终止角度*/270, /*线宽*/2, /*圆心坐标X*/ui_x_debug_color+40, /*圆心坐标Y*/ui_y_debug_color, 0/*无*/, 20/*x半轴长度*/, 20/*y半轴长度*/);

    //画右前轮-圆弧
    Client_UIColor_Decision(G_system_monitor.CAN2_rx_fps, 0, G_system_monitor.CAN2_rx_fps, 0);
    pictureOperation(&G_sentry.G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[4], UI_Debug[4], Command, /*圆弧*/4,  8,  en_Green, 
        /*起始角度*/90, /*终止角度*/270, /*线宽*/2, /*圆心坐标X*/ui_x_debug_color+140, /*圆心坐标Y*/ui_y_debug_color, 0/*无*/, 20/*x半轴长度*/, 20/*y半轴长度*/);
    
    //画电容-矩形
    Client_UIColor_Decision(G_system_monitor.CAN1_rx_fps, 0, G_system_monitor.CAN1_rx_fps, 0);
    pictureOperation(&G_sentry.G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[5], UI_Debug[5], Command, /*矩形*/1,  8,  ui_color, 
        /*无*/0, /*无*/0, /*线宽*/2, /*起点坐标X*/ui_x_debug_color+20, /*起点坐标Y*/ui_y_debug_color+20, 0/*无*/, ui_x_debug_color+60/*对角顶点x*/, ui_y_debug_color+40/*对角顶点y*/);

    //画裁判系统-矩形
    Client_UIColor_Decision(G_system_monitor.UART2_rx_fps, 40, G_system_monitor.UART2_rx_fps, 4);
    pictureOperation(&G_sentry.G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[6], UI_Debug[6], Command, /*矩形*/1,  8,  ui_color, 
        /*无*/0, /*无*/0, /*线宽*/2, /*起点坐标X*/ui_x_debug_color+160, /*起点坐标Y*/ui_y_debug_color+50, 0/*无*/, ui_x_debug_color+190/*对角顶点x*/, ui_y_debug_color+60/*对角顶点y*/);

}

uint32_t ui_shooter_width = 6;
uint32_t ui_pitch_radius = 30;
void SentryRobot::Client_Prepare_Char_DebugData_2(u8 Command)//
{
    //画Yaw轴-矩形
    Client_UIColor_Decision(G_system_monitor.Gimbal_Yaw_Receive_fps, 50, G_system_monitor.Gimbal_Yaw_Receive_fps, 50);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[0], UI_Debug[7], Command, /*矩形*/1,  8,  ui_color, 
        /*无*/0, /*无*/0, /*线宽*/2, /*起点坐标X*/ui_x_debug_color+70, /*起点坐标Y*/ui_y_debug_color+50, 0/*无*/, ui_x_debug_color+130/*对角顶点x*/, ui_y_debug_color+70/*对角顶点y*/);

    //画云台-矩形
//    Client_UIColor_Decision(system_monitor.CAN_Rx_SupplyPellet_fps, 900, system_monitor.CAN_Tx_SupplyPellet_fps, 900);
	ui_color = en_Green;
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[1], UI_Debug[8], Command, /*矩形*/1,  8,  ui_color, 
        /*无*/0, /*无*/0, /*线宽*/2, /*起点坐标X*/ui_x_debug_color+50, /*起点坐标Y*/ui_y_debug_color+70, 0/*无*/, ui_x_debug_color+150/*对角顶点x*/, ui_y_debug_color+120/*对角顶点y*/);

    //画Pitch轴-圆弧
    Client_UIColor_Decision(G_system_monitor.Gimbal_Yaw_Receive_fps, 50 ,G_system_monitor.Gimbal_Yaw_Receive_fps, 50);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[2], UI_Debug[9], Command, /*圆弧*/4,  8,  ui_color, 
        /*起始角度*/240, /*终止角度*/120, /*线宽*/2, /*圆心坐标X*/ui_x_debug_color+100, /*圆心坐标Y*/ui_y_debug_color+120+ui_pitch_radius/2, 0/*无*/, ui_pitch_radius/*x半轴长度*/, ui_pitch_radius/*y半轴长度*/);

    //画枪管及云控-直线
    Client_UIColor_Decision(G_system_monitor.UART4_rx_fps,50, G_system_monitor.UART4_rx_fps, 50);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[3], UI_Debug[10], Command, /*直线*/0,  8,  ui_color, 
        /*无*/0, /*无*/0, /*线宽*/ui_shooter_width, /*起点坐标X*/ui_x_debug_color+130, /*起点坐标Y*/ui_y_debug_color+120+ui_pitch_radius/2, 0/*无*/, ui_x_debug_color+180/*对角顶点x*/, ui_y_debug_color+120+ui_pitch_radius/2/*对角顶点y*/);

    //画右摩擦轮-矩形
    Client_UIColor_Decision(G_system_monitor.UART3_rx_fps, 50, G_system_monitor.UART3_rx_fps, 50);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[4], UI_Debug[11], Command, /*矩形*/1,  8,  ui_color, 
        /*无*/0, /*无*/0, /*线宽*/3, /*起点坐标X*/ui_x_debug_color+140, /*起点坐标Y*/ui_y_debug_color+120+ui_pitch_radius/2-ui_shooter_width/2, 0/*无*/, ui_x_debug_color+160/*对角顶点x*/, ui_y_debug_color+120+ui_pitch_radius/2-5-ui_shooter_width/2/*对角顶点y*/);
    
    //画左摩擦轮-矩形
    Client_UIColor_Decision(G_system_monitor.UART1_rx_fps, 50, G_system_monitor.UART1_rx_fps,50);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[5], UI_Debug[12], Command, /*矩形*/1,  8,  ui_color, 
        /*无*/0, /*无*/0, /*线宽*/3, /*起点坐标X*/ui_x_debug_color+160, /*起点坐标Y*/ui_y_debug_color+120+ui_pitch_radius/2-ui_shooter_width/2, 0/*无*/, ui_x_debug_color+170/*对角顶点x*/, ui_y_debug_color+120+ui_pitch_radius/2-5-ui_shooter_width/2/*对角顶点y*/);

}
u8 floatData1Name[4] = "FP1";
u8 floatData2Name[4] = "FP2";
u8 floatData3Name[4] = "FP3";
u8 floatData4Name[4] = "FP4";
u8 floatData5Name[4] = "FP5";
u8 floatData6Name[4] = "FP6";
u8 floatData7Name[4] = "FP7";
u8* floatDataName[] = {floatData1Name, floatData2Name, floatData3Name, floatData4Name, floatData5Name, floatData6Name, floatData7Name};
void SentryRobot::Client_Prepare_Normal_Data(u8 Command)
{
    float Client_VolumeY_Top_0 = 270;
    float Client_VolumeX_Top_0 = 580;
    float Client_VolumeY_Top_20 = 100;
    float Client_VolumeX_Top_20 = 190;
    float Client_VolumeX_Koffset_Top = 1.05;

    Client_Sendata_Coe.Volume_X0 = Client_VolumeX_Top_0;
    Client_Sendata_Coe.Volume_X1 = Client_VolumeX_Top_20;
    Client_Sendata_Coe.Volume_Y0 = Client_VolumeY_Top_0;
    Client_Sendata_Coe.Volume_Y1 = Client_VolumeY_Top_20;
    Client_Sendata_Coe.Volume_K	 = Client_VolumeX_Koffset_Top;
    Client_Sendata_Coe.Volume_Angle_Inc = 20;
    //线性插值并根据角度算出横纵基准
    Client_Sendata_Coe.Volume_X =  Client_Sendata_Coe.Volume_X0 +G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current*(Client_Sendata_Coe.Volume_X1-Client_Sendata_Coe.Volume_X0)/Client_Sendata_Coe.Volume_Angle_Inc;
    Client_Sendata_Coe.Volume_Y =  Client_Sendata_Coe.Volume_Y0 +G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current*(Client_Sendata_Coe.Volume_Y1-Client_Sendata_Coe.Volume_Y0)/Client_Sendata_Coe.Volume_Angle_Inc;
        
    //从左下开始，逆时针画出各条直线
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[1], floatDataName[1], Command, 0, 7, 0, 0, 0, 
    /*线宽*/10, /*起点坐标X*/(960-Client_Sendata_Coe.Volume_X), /*起点坐标Y*/0 , 0,
    /*终点坐标X*/(960-(Client_Sendata_Coe.Volume_X-Client_Sendata_Coe.Volume_K*Client_Sendata_Coe.Volume_Y)),/*终点坐标Y*/Client_Sendata_Coe.Volume_Y);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[2], floatDataName[2], Command, 0, 7, 0, 0, 0, 
    /*线宽*/10, /*起点坐标X*/(960+(Client_Sendata_Coe.Volume_X-Client_Sendata_Coe.Volume_K*Client_Sendata_Coe.Volume_Y)), /*起点坐标Y*/Client_Sendata_Coe.Volume_Y , 0,  /*终点坐标X*/(960+Client_Sendata_Coe.Volume_X),/*终点坐标Y*/0);

}


//绘制对方机器人的血量,这样可以让操作手更直观的看到血量
u8 Enemy_HP_1[4] = "En1";
u8 Enemy_HP_2[4] = "En2";
u8 Enemy_HP_3[4] = "En3";
u8 Enemy_HP_4[4] = "En4";
u8 Enemy_HP_5[4] = "En5";
u8 *Enemy_HP[] = {Enemy_HP_1, Enemy_HP_2, Enemy_HP_3, Enemy_HP_4, Enemy_HP_5};
uint32_t X_mid = 960;
uint32_t Y_mid = 900;
u16 enemy_hp;
u16 enemy_hp_test=900;
u8 red_or_blue;
void SentryRobot::Client_Enemy_HP(u8 Command)
{
    if(G_referee.GameRobotStatus.robot_id<=50)
    {
        red_or_blue=0;
    }
    else
    {
        red_or_blue=1;
    }

    for(int cnt_i=1;cnt_i<=5;cnt_i++)
    {
        enemy_hp = ( (u16 *)(&G_referee.GameRobotHP) )[ ( (cnt_i-1) + 8*red_or_blue) ];
        pictureOperation(&G_ST_Client_Custom_Graphic_Five.grapic_data_struct[cnt_i-1], Enemy_HP[cnt_i-1], Command, /*整数*/6, 1, en_Orange,
            /*字体大小*/16, /*有效小数位*/0, /*线宽*/3, /*起点坐标X*/X_mid + (red_or_blue==0?-1:1) * (40+120*cnt_i),/*起点坐标Y*/Y_mid ,enemy_hp ,enemy_hp>>10,enemy_hp>>21 );
            
        //UI写数字的测试函数
        //pictureOperation(&G_ST_Client_Custom_Graphic_Five.grapic_data_struct[cnt_i-1], Enemy_HP[cnt_i-1], Command, /*整数*/6, 1, en_Orange,
        //    /*字体大小*/16, /*有效小数位*/0, /*线宽*/3, /*起点坐标X*/X_mid + (red_or_blue==0?-1:1) * (40+120*cnt_i),/*起点坐标Y*/Y_mid ,enemy_hp_test ,enemy_hp_test>>10,enemy_hp_test>>21 );
    
    }
}


uint32_t ui_radar_x_base = 810;
uint32_t ui_radar_y_base = 825;
u8 UI_Radar_Warming_0[4] = "WR0";
u8 UI_Radar_Warming_1[4] = "WR1";
u8 UI_Radar_Warming_2[4] = "WR2";
u8 *UI_Radar_Warming[] = {UI_Radar_Warming_0, UI_Radar_Warming_1, UI_Radar_Warming_2};
u8 UI_Radar_Message_1[30] = "Enemy Hero Come!\n";
u8 UI_Radar_Message_2[30] = "Enemy Dart Open!\n";
u8 UI_Radar_Message_3[30] = "   Enemy Fly!\n";
u8 *UI_Radar_Message[] = {UI_Radar_Message_1, UI_Radar_Message_2, UI_Radar_Message_3};

void SentryRobot::Client_Draw_Radar_Message(u8 command)
{
   static u8 cnt_i = 0;//取值在0，1，2之间循环，表示u8的3位数
		//将对应的雷达信息填入
   for(int cnt_j=0;cnt_j<30;cnt_j++)
   {
      G_ST_Client_Custom_Character.data[cnt_j] = ((u8 *)UI_Radar_Message[cnt_i])[cnt_j];
    }
		
    pictureOperation(&G_ST_Client_Custom_Character.grapic_data_struct, UI_Radar_Warming[cnt_i], command/*添加*/, 7/*字符*/, 0/*图层*/, en_Yellow,
            /*字体大小*/20, /*字符长度*/30, /*线宽*/4, /*起点坐标X*/ui_radar_x_base, /*起点坐标Y*/ui_radar_y_base+30*cnt_i , 0,0,0);
		
    cnt_i = cnt_i>=2?0:cnt_i+1;
}

u8 UI_Console_0[4] = "C00";
u8 UI_Console_1[4] = "C01";
u8 UI_Console_2[4] = "C02";
u8 UI_Console_3[4] = "C03";
u8 UI_Console_4[4] = "C04";
u8 UI_Console_5[4] = "C05";
u8 UI_Console_6[4] = "C06";
u8 UI_Console_7[4] = "C07";
u8 UI_Console_8[4] = "C08";
u8 UI_Console_9[4] = "C09";
u8 UI_Console_10[4] = "C10";
u8 *UI_Console[] = {UI_Console_0, UI_Console_1, UI_Console_2, UI_Console_3, UI_Console_4,
                    UI_Console_5, UI_Console_6, UI_Console_7, UI_Console_8, UI_Console_9, UI_Console_10};
//速度表绘制
void SentryRobot:: Client_Draw_Console(u8 Command)
{

    uint32_t ui_x_base_1 = 650;
    uint32_t ui_y_base_1 = 500;
    uint32_t speed_pointer_x = 0;
    uint32_t speed_pointer_y = 0;
    uint32_t ui_x_base_2 = 1650;
    uint32_t ui_y_base_2 = 400;
		
      speed_pointer_x = (uint32_t)(ui_x_base_1 + 80.0f*sin(3.14f*(G_sentry.m_chassis_speed/3600)*60.0f/180.0f));
      speed_pointer_y = (uint32_t)(ui_y_base_1 + 80.0f*cos(3.14f*(G_sentry.m_chassis_speed/3600)*60.0f/180.0f));

    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[4], UI_Console[8], Command, /*直线*/0,  4,  0/*己方颜色*/, 
        /*无*/0, /*无*/0, /*线宽*/4, /*起点坐标X*/ui_x_base_1, /*起点坐标Y*/ui_y_base_1, 0/*无*/, speed_pointer_x/*终点坐标x*/, speed_pointer_y/*终点坐标y*/);
    pictureOperation(&G_ST_Client_Custom_Graphic_Seven.grapic_data_struct[5], UI_Console[9], Command, /*浮点数*/5,  4,  0/*己方颜色*/, 
        /*字体大小*/20, /*有效小数位*/0, /*线宽*/2, /*起点坐标X*/ui_x_base_1-60, /*起点坐标Y*/ui_y_base_1-30, (float)G_sentry.m_chassis_speed, (int)G_sentry.m_chassis_speed>>10, (int)G_sentry.m_chassis_speed>>21);
}

//TODO:[打弹热量控制]测试版（参考老步兵代码逻辑）
/*----------------------------------------------------------------------------------------
函数名：bool Is_Heat_Safe(void)
功能： 判断枪口热量是否安全
备注： 1-可以拨弹 0-不能拨弹
//枪口热量规则：
//1. 每发射一颗射速为Q1（Q1<25m/s）的小弹丸，枪口热量增加10，数据更新频率50Hz，热量结算10Hz；
----------------------------------------------------------------------------------------*/
bool SentryRobot:: Is_Heat_Safe(void)
{
    //s32 Heatlimit =  (ShooterHeat_Limit-20) <=(0.6f*ShooterHeat_Limit)? (0.6f*ShooterHeat_Limit):(ShooterHeat_Limit-20); //枪口热量上限
    if(ShooterHeat_Limit<110 )
        Heatlimit =  ShooterHeat_Limit-35; 
    else
        Heatlimit =  ShooterHeat_Limit-40;
      
    static float Pre_ShooterHeat = 0.0f;    //记录上一次枪口热量
    //static float ShooterPosDes_Pre = 0.0f;

    // if(G_system_monitor.UART2_rx_fps>30)         //裁判系统帧率正常
    if (G_system_monitor.Referee_Receive_fps > 30) //裁判系统帧率正常
    {
        //如果在打弹（弹速不会稳定在固定值）
        if(PelletSpeed_mes != G_sentry.Shoot_Heat_Control.Pre_ShootSpeed)
        {
            G_sentry.Shoot_Heat_Control.Allowed_PelletNum--;    //发射速度更新，说明打出弹丸，计数
            G_sentry.Shoot_Heat_Control.PelletNum++;            //已经发射子弹颗数
        }
        //如果在不打弹（会有冷却）

        else if(Is_Float_Equal(ShooterHeat_mes, Pre_ShooterHeat)==false ||
                Is_Float_Equal(ShooterHeat_mes, 0.0f)==true)              //枪口热量更新周期为20ms
        {
            G_sentry.Shoot_Heat_Control.Allowed_PelletNum =(s16)((Heatlimit - ShooterHeat_mes) / 10); //一个周期更新会计算该周期可发射弹丸数量
        }

        Pre_ShooterHeat = ShooterHeat_mes;  //记录枪口热量
        G_sentry.Shoot_Heat_Control.Pre_ShootSpeed = PelletSpeed_mes;   //记录弹丸速度
        G_sentry.Shoot_Heat_Control.Heat_Left = (float)Heatlimit - (float)ShooterHeat_mes;
    }
    //接收帧率不正常（类裁判系统进行热量以及冷却值的计算）
    else
    {
        G_sentry.Shoot_Heat_Control.shoot_num = (G_sentry.shoot_driver_angle_target-G_sentry.Shoot_Heat_Control.ShooterPosDes_Pre) / (float)SHOOT_DRIVER_SUPPLY_ANGLE_STEP;
        G_sentry.Shoot_Heat_Control.Heat_Left -=10.0f *  G_sentry.Shoot_Heat_Control.shoot_num;

        G_sentry.Shoot_Heat_Control.Heat_Left += ShooterHeat_Coll/100.0f;
        G_sentry.Shoot_Heat_Control.Heat_Left = G_sentry.Shoot_Heat_Control.Heat_Left<(float)Heatlimit ? G_sentry.Shoot_Heat_Control.Heat_Left:(float)Heatlimit;
        G_sentry.Shoot_Heat_Control.Heat_Left = G_sentry.Shoot_Heat_Control.Heat_Left>0.0f ? G_sentry.Shoot_Heat_Control.Heat_Left:0.0f;    //避免出现负数u8类型溢出
        G_sentry.Shoot_Heat_Control.Allowed_PelletNum = (s16)(G_sentry.Shoot_Heat_Control.Heat_Left/10.0f);
        G_sentry.Shoot_Heat_Control.ShooterPosDes_Pre = G_sentry.shoot_driver_angle_target;
    }

    if( G_sentry.Shoot_Heat_Control.Allowed_PelletNum<=1||G_system_monitor.Gimbal_CAN2_fps<=0 )  
        return false;  //不允许发送
    else                        
        return true;            //允许发射
}
